use cmsis_rtos2_rs::static_mem::{MsgQueue, Task, Thread};
use proto_bytes::{frame::{Proto, Res2}, message::{Message, ReqRef}, package::ty};

use crate::{cmd, conn::write_wait_ack, error::AppErr};

use super::handler;

static THREAD: Thread<2048> = Thread::uninit();
struct RequestTask;

static MSG_QUEUE: MsgQueue<Message, 2> = MsgQueue::uninit();

pub unsafe fn start() {
    MSG_QUEUE.init_once();
    THREAD.spawn(&RequestTask);
}

pub fn post(req: ReqRef) -> bool {
    let cmd = req.cmd();
    match cmd {
        cmd::heat::PEARL_MOTOR => {
            _ = MSG_QUEUE.post(req.to_owned());
            true
        }
        _ => false,
    }
}

impl Task for RequestTask {
    fn run(&self) {
        loop {
            let msg = MSG_QUEUE.take();
            dispatch(msg.as_ref().into_req());
        }
    }
}

fn dispatch(req: ReqRef) {
    let cmd = req.cmd();
    let ret = match cmd {
        cmd::heat::PEARL_MOTOR => req
            .parse()
            .ok_or(AppErr::Parse)
            .and_then(|req| handler::on_pearl_motor(req)),

        _ => Err(AppErr::InvalidCmd),
    };

    write_wait_ack(Proto {
        ty: ty::RES,
        seq: req.seq(),
        src: req.dst(),
        dst: req.src(),
        body: Res2 {
            cmd: req.cmd(),
            ret,
        }
    });
}
